#region Libraries
// Main libraries:
using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Ccr.Adapters.Wpf;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using System.Windows;
using System.Xml;

// Sensors:
using brick = Microsoft.Robotics.Services.Sample.Lego.Nxt.Brick.Proxy;
using lightsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.LightSensor.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using console = Microsoft.Dss.Services.ConsoleOutput.Proxy;
#endregion

namespace LabyRob
{
	/// <summary>
	/// The service
	/// </summary>
	[Contract(Contract.Identifier)]
	[DisplayName("(User) LabyRob")]
	[Description("Lego Mindstorms labyrinth navigation")]
	[DssServiceDescription("http://labyrob.codeplex.com")]
	public class LabyRobService : DsspServiceBase
	{

		#region Properties
		private WpfServicePort _wpfServicePort;
		private Dashboard _dashboard;
		private Queue<LogEntry> _logEntries;
		private bool canAjustSpeed = true;
		#endregion

		#region Service State/Port
		[ServiceState]
		LabyRobState _state = new LabyRobState();
		
		[ServicePort("/LabyRob", AllowMultipleInstances = true)]
		LabyRobOperations _mainPort = new LabyRobOperations();
		#endregion

		#region Partners definitions
		// Brick:
		/// Brick partner
		[Partner("NxtBrick", Contract = brick.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
		brick.NxtBrickOperations _nxtBrickPort = new brick.NxtBrickOperations();
		brick.NxtBrickOperations _nxtBrickNotify = new brick.NxtBrickOperations();

		// Light sensor:
		[Partner("NxtLightSensor", Contract = lightsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
		lightsensor.LightSensorOperations _nxtLightSensorPort = new lightsensor.LightSensorOperations();
		lightsensor.LightSensorOperations _nxtLightSensorNotify = new lightsensor.LightSensorOperations();

		// Wheels:
		[Partner("Drive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
		drive.DriveOperations _drivePort = new drive.DriveOperations();
		drive.DriveOperations _driveNotify = new drive.DriveOperations();

		// Console:
		[Partner("Console", Contract = console.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
		console.ConsoleOutputPort _consolePort = new console.ConsoleOutputPort();
		console.ConsoleOutputPort _consoleNotify = new console.ConsoleOutputPort();
		#endregion
		
		/// <summary>
		/// Class constructor
		/// </summary>
		/// <param name="creationPort">Port</param>
		public LabyRobService(DsspServiceCreationPort creationPort)
			: base(creationPort)
		{
			_logEntries = new Queue<LogEntry>();
		}

		#region Initiation mathods
		/// <summary>
		/// Starts service and dashboard
		/// </summary>
		protected override void Start()
		{
			SpawnIterator(InitDashboard);
			InitService();
		}

		/// <summary>
		/// Initialtes the service
		/// </summary>
		protected void InitService()
		{
			// Subscribe notifications from ports:
			_nxtBrickPort.Subscribe(_nxtBrickNotify);
			_nxtLightSensorPort.Subscribe(_nxtLightSensorNotify);
			_drivePort.Subscribe(_driveNotify);
			_consolePort.Subscribe(_consoleNotify);

			// Set concurent action handlers for notifications:
			Activate(Arbiter.Interleave(
				new TeardownReceiverGroup(),
				new ExclusiveReceiverGroup(),
				new ConcurrentReceiverGroup(
					Arbiter.Receive<brick.ConnectToHardware>(false, _nxtBrickNotify, BrickHandler),
					Arbiter.Receive<brick.DisconnectFromHardware>(false, _nxtBrickNotify, BrickHandlerFail),
					Arbiter.Receive<lightsensor.Replace>(true, _nxtLightSensorNotify, LightHandler),
					Arbiter.Receive<console.Insert>(true, _consoleNotify, ConsoleHandler)
					//Arbiter.Receive<drive.Update>(true, _driveNotify, basd)
				)
			));
			Activate(Arbiter.ReceiveWithIterator<drive.Update>(true, _driveNotify, DriveUpdateHandler));
			Activate(Arbiter.ReceiveWithIterator<drive.RotateDegrees>(true, _driveNotify, RotateDegreesUpdateHandler));

			// Start the service:
			base.Start();
		}

		/// <summary>
		/// wefwe
		/// </summary>
		/// <param name="not"></param>
		public void basd(drive.Update not) 
		{
			Console.WriteLine(not.Body.RightWheel);
		}

		/// <summary>
		/// Starts the dashboard
		/// </summary>
		/// <returns>Window</returns>
		protected IEnumerator<ITask> InitDashboard()
		{
			// Init window:
			_wpfServicePort = WpfAdapter.Create(TaskQueue);
			var window = _wpfServicePort.RunWindow(() => new Dashboard(this));
			yield return (Choice) window;
			// Get exception if any:
			var exception = (Exception)window;
			if (exception != null)
			{
				LogError(exception);
				StartFailed();
				yield break;
			}
			// Need double cast because WPF adapter doesn't know about derived window types:
			_dashboard = (Window)window as Dashboard;
		}

		/// <summary>
		/// Shutdowns the service
		/// </summary>
		public void Exit()
		{
			ControlPanelPort.Shutdown();
		}
		#endregion

		#region Handlers
		/// <summary>
		/// Drive notification
		/// </summary>
		/// <param name="notification"></param>
		/// <returns></returns>
		private IEnumerator<ITask> DriveUpdateHandler(drive.Update notification)
		{
			drive.DriveDifferentialTwoWheelState s = notification.Body;
			Console.WriteLine(s.RightWheel);
			Console.WriteLine(s.RightWheel);
			yield break;
		}

		/// <summary>
		/// Drive rotation notification
		/// </summary>
		/// <param name="notification"></param>
		/// <returns></returns>
		private IEnumerator<ITask> RotateDegreesUpdateHandler(drive.RotateDegrees notification)
		{
			Console.WriteLine(notification.Body.RotateDegreesStage);
			yield break;
		}

		/// <summary>
		/// Connect to hardware event
		/// </summary>
		/// <param name="notification">Connect to Hardware event</param>
		protected void BrickHandler(brick.ConnectToHardware notification)
		{
			AddLogEntry(new LogEntry("Connected to NXT device", new XmlQualifiedName("info"), DateTime.Now));
		}

		/// <summary>
		/// Disconnect to hardware event
		/// </summary>
		/// <param name="notification">Connect to Hardware event</param>
		protected void BrickHandlerFail(brick.DisconnectFromHardware notification)
		{
			AddLogEntry(new LogEntry("Disconnected from NXT device", new XmlQualifiedName("none"), DateTime.Now));
			lightsensor.SpotlightRequest spotlightRequest = new lightsensor.SpotlightRequest();
			spotlightRequest.SpotlightOn = true;
			_nxtLightSensorPort.Spotlight(spotlightRequest);
		}

		/// <summary>
		/// Console output handler
		/// </summary>
		/// <param name="notification">Received notification</param>
		protected void ConsoleHandler(console.Insert notification)
		{
			// If dashboard isn't initiated yet:
			if (_dashboard == null)
			{
				// Save the log entry to the buffer to use later when we have a dashboard:
				_logEntries.Enqueue(new LogEntry(notification.Body.Subject, notification.Body.Level, notification.Body.Time));
			}
			else
			{
				
			}
		}

		/// <summary>
		/// Light/color handler
		/// </summary>
		/// <param name="notification">Received notification</param>
		protected void LightHandler(lightsensor.Replace notification)
		{
			int light = notification.Body.Intensity;
			//drive.SetDrivePowerRequest driveRequest = new drive.SetDrivePowerRequest();
			drive.RotateDegreesRequest rotateRequest = new drive.RotateDegreesRequest();

			//System.Console.WriteLine(light.ToString());

			/*_wpfServicePort.Invoke(() =>
			{
				_dashboard.alert(light.ToString());
			});*/

			if (light == 1)
			{
				SpawnIterator<double, double>(0.5, 0.5, Drive);
				canAjustSpeed = true;
				//driveRequest.LeftWheelPower = 0.5;
				//driveRequest.RightWheelPower = 0.5;
				//_drivePort.SetDrivePower(driveRequest);

			}
			else
			{
				canAjustSpeed = false;
				SpawnIterator<double, double>(0, 0, Drive);
				/*
				/*SpawnIterator<double, double>(0, 0.0, MakeTurn);
				if (light == 1) {
					driveRequest.LeftWheelPower = 1.0;
					driveRequest.RightWheelPower = 1.0;
				}
				else
				{
				drive.DriveDistanceRequest distanceRequest = new drive.DriveDistanceRequest();
				//distanceRequest.Distance = 0.03;
				//distanceRequest.Power = 1.0;
				//_drivePort.DriveDistance(distanceRequest);
				driveRequest.LeftWheelPower = 0.0;
				driveRequest.RightWheelPower = 0.0;
				_drivePort.SetDrivePower(driveRequest);
				//}*/
			}

		}
		#endregion

		#region Action methods

		/// <summary>
		/// Drive robot
		/// </summary>
		/// <param name="leftPower"></param>
		/// <param name="rightPower"></param>
		/// <returns></returns>
		protected IEnumerator<ITask> Drive(double leftPower, double rightPower)
		{
			drive.SetDrivePowerRequest driveRequest = new drive.SetDrivePowerRequest(leftPower, rightPower);
			yield return Arbiter.Choice(_drivePort.SetDrivePower(driveRequest),
				delegate(DefaultUpdateResponseType response) {
					Console.WriteLine(response.ToString());
				},
				delegate(Fault fault) { Console.WriteLine("Unable to drive " + fault); }
			);
		}



		/// <summary>
		/// Turns robot by specified angle with specified power
		/// </summary>
		/// <param name="degree">Angle</param>
		/// <param name="power">Power</param>
		/// <returns></returns>
		protected IEnumerator<ITask> MakeTurn(double degree, double power)
		{
			drive.RotateDegreesRequest rotateRequest = new drive.RotateDegreesRequest();
			rotateRequest.Degrees = degree;
			rotateRequest.Power = power;
			yield return Arbiter.Choice(_drivePort.RotateDegrees(rotateRequest),
				delegate(DefaultUpdateResponseType response) { },
				delegate(Fault fault) { Console.WriteLine("Unable to rotate " + fault); }
			);
		}
		/// <summary>
		/// 
		/// </summary>
		public void asd() {
			SpawnIterator<double, double>(350, 0.9, MakeTurn);
		}
		#endregion

		#region Interface methods
		/// <summary>
		/// Sets power of both wheels
		/// </summary>
		/// <param name="left">Left wheel power</param>
		/// <param name="right">Right wheel power</param>
		public void SetWheelsPower(double left = 1.0, double right = 1.0)
		{
			if (canAjustSpeed)
			{
				// Check range:
				if (left < 0)
					left = 0;
				if (right < 0)
					right = 0;
				if (left > 1)
					left = 1.0;
				if (right > 1)
					right = 1;

				// Set power:
				_state.leftWheelPower = left;
				_state.rightWheelPower = right;

				// Update power:
				drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
				powerRequest.LeftWheelPower = left;
				powerRequest.RightWheelPower = right;
				_drivePort.SetDrivePower(powerRequest);
			}
		}

		/// <summary>
		/// Drive with specified power
		/// </summary>
		/// <param name="left">Left wheel power</param>
		/// <param name="right">Right wheel power</param>
		public void DoDrive(double left, double right)
		{
			drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
			powerRequest.LeftWheelPower = left;
			powerRequest.RightWheelPower = right;
			_drivePort.SetDrivePower(powerRequest);
		}

		/// <summary>
		/// Adds log entry and all old entries
		/// </summary>
		/// <param name="log">Log entry</param>
		protected void AddLogEntry(LogEntry log)
		{
			_wpfServicePort.Invoke(() =>
			{
				// If we have something in the buffer, prepend it first and dalete then:
				while (_logEntries.Count > 0)
					_dashboard.addLogItem(_logEntries.Dequeue());
				// Add the log entry to the dashboard console:
				_dashboard.addLogItem(new LogEntry(log.subject, log.type, log.time));
			});
		}
		#endregion
	}
}


